#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
#include <nav_msgs/Odometry.h>

std::string turtle_name;

void poseCallback(const turtlesim::Pose::ConstPtr& msg)
{
    // 该回调函数中根据乌龟当前的位姿，发布小乌龟相对于世界坐标系的坐标变换
    // 定义tf广播器
    static tf::TransformBroadcaster br;
    //定义一个tf关系
    tf::Transform transform;
    //将小乌龟当前的位置设置为小乌龟坐标系的原点
    transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0));
    //定义一个四元数
    tf::Quaternion q;
    //用小乌龟当前的姿态设置小乌龟坐标系的旋转
    q.setRPY(0, 0, msg->theta);
    transform.setRotation(q);
    // 发布坐标变换
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
int main(int argc, char** argv)
{
    // 初始化节点
    ros::init(argc, argv, "robot_tf_broadcaster");
    //从参数服务器中获取小乌龟的名字
    ros::param::get("~turtle_name",turtle_name);
    // 创建ros句柄
    ros::NodeHandle node;
    //订阅小乌龟当前的位置信息
    ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
    //阻塞程序等待回调函数
    ros::spin();
    return 0;
};